﻿#-------------------------------------------------------------------------------
# Name:        module1
# Purpose:
#
# Author:      Patrick Sanchez, Marcel Carrere
#
# Created:     28/05/2011
# Copyright:   (c) BDI 2011
# Licence:     <gnu>
#-------------------------------------------------------------------------------
#!/usr/bin/env python
import numpy as np
import pylab as mp
import scipy as sc
from scipy import *
from scipy.fftpack import fft, ifft, fftshift
from scipy.io.matlab import mio
from scipy.signal import resample
import pylab as plt

def distance(ref):
    distance=[]

    coordonnees_X=[-0.0307,0.0315,-0.0529,0.0549,-0.0672,0.0691,-0.0551,0.0573,-0.0308,0.0312,-0.0742,0.076,-0.0874,0.0882,-0.0756,0.0762,0.0004,0.0005,0.0004,0.0002,-0.0347,0.0358,-0.0371,0.0399,-0.0809,0.084,-0.0828,0.0862,-0.083,0.090,-0.0285,0.031,-0.038,0.0398,-0.0296,0.0321,-0.0355,0.0377,-0.0631,0.0647,-0.0657,0.068,-0.0396,0.0383,-0.0674,0.07,-0.0849,0.0863,-0.0705,0.0722,-0.0574,0.0579,-0.0836,0.085,-0.0879,0.087,-0.052,0.050,0.0002,0.0005,0.0003,0.0005]
    coordonnees_Y=[0.0949,0.0964,0.052,0.0531,-0.0228,-0.0219,-0.0926,-0.0927,-0.1188,-0.1188,0.052,0.0536,-0.0117,-0.0106,-0.0749,-0.0749,0.0524,-0.0275,-0.1005,-0.1226,0.0151,0.0158,-0.0668,-0.0663,0.0185,0.0199,-0.0531,-0.0524,-0.05,-0.05,0.0521,0.0527,-0.0267,-0.026,-0.0983,-0.0986,0.0824,0.0831,0.0165,0.0176,-0.061,-0.0601,-0.1123,-0.1125,0.052,0.0534,-0.0176,-0.0166,-0.0843,-0.0843,0.0793,0.0803,0.0209,0.0224,-0.0445,-0.0435,-0.11,-0.11,0.0983,-0.0682,-0.1159,0.0148]
    coordonnees_Z=[0.014,0.0141,0.0567,0.0554,0.0649,0.064,0.0425,0.0426,-0.011,-0.0111,0.0031,0.0029,-0.0062,-0.0063,-0.012,-0.0122,0.0792,0.0969,0.0669,-0.0073,0.0873,0.0858,0.0837,0.0836,0.0335,0.0333,0.026,0.0267,-0.015,-0.015,0.0741,0.0732,0.0908,0.0892,0.0602,0.0608,0.041,0.0414,0.0644,0.0634,0.0589,0.0585,0.0204,0.0194,0.0313,0.0308,0.0316,0.0314,0.017,0.0169,0.0086,0.0085,-0.0018,-0.002,-0.0101,-0.01,-0.012,-0.012,0.0186,0.0902,0.0307,0.0955]
    numvoie=0
    while numvoie<62:
        distance[numvoie]=np.sqrt(((coordonnees_X[numvoie]-coordonnees_X[ref])*(coordonnees_X[numvoie]-coordonnees_X[ref]))+((coordonnees_Y[numvoie]-coordonnees_Y[ref])*(coordonnees_Y[numvoie]-coordonnees_Y[ref]))+((coordonnees_Z[numvoie]-coordonnees_Z[ref])*(coordonnees_Z[numvoie]-coordonnees_Z[ref])))
        numvoie=numvoie+1

    return distance


def mat2pydata(filename):
    """convert mat file to python data
     input : matlab file
     output : nb. channel : integer
              block size  : integer
              nb. block   : integer
              channels    : matrix of signals
              samplig rate : real
              dt : sampling period : real
     """
    data=[]
    del (data)
    data = mio.loadmat(filename)        # lecture mat file -V6
    srate = data['srate']               # recup frequence echantillonnage
    blocksize=data['blocksize']
    channels = data['voies']            # recup voies acquisition
    nbchannel, sizefull=channels.shape   # taille de la matrice
    nbblock=sizefull/blocksize
    channels=channels.reshape((nbchannel,nbblock,blocksize))
    dt=1.0/srate
    return nbchannel,nbblock, blocksize ,channels,srate, dt

def BlockBeforeTMS(channels,blocksize,srate,temps,ThresholdBeforeTMS,deltaBeforeTMS):
    """ select block of data before TMS with take care of time before TMS
    input : channels : matrix of signals
            blocksize : integer
            srate : integer sampling rate
            time : numpy array of time
            ThresholdBeforeTMS : float threshold before TMS in second
            deltaBeforeTMS : float buffer size before TMS
    output : matrix of data without TMS*
            t : float array, new time without TMS
    """
    i=int(ThresholdBeforeTMS*srate)
    j=int((deltaBeforeTMS+ThresholdBeforeTMS)*srate)
    l1=int(blocksize/2) - i
    l0=int(blocksize/2) - j
    M=channels[l0:l1,:]
    t=temps[l0:l1]
    t=np.reshape(t, l1-l0)
    t0=temps[l0]
    t1=temps[l1]
    return M,t, l0, l1, t0, t1


def BlockAfterTMS(channels,blocksize,srate,temps,ThresholdAfterTMS,deltaAfterTMS):
    """select block after TMS with care of time before TMS
    input: matrix of signals
    blocksize:integer
    srate:sampling rate
    time: numpy array of time
    ThresholdAfterTMS: float threshold after TMS in second
    deltaAfterTMS: buffer size after TMS
    ouput : matrix of data without TMS
    t:float array, new time without TMS
    """
    i=int(ThresholdAfterTMS*srate)
    j=int((deltaAfterTMS+ThresholdAfterTMS)*srate)
    l0=int(blocksize/2)+i
    l1=int(blocksize/2)+j
    M=channels[ l0:l1, :]
    t=np.zeros(l1-l0, dtype="float32")
    t=temps[l0: l1]
    t=np.reshape(t, l1-l0)
    t0=temps[l0]
    t1=temps[l1]
    return M,t, l0, l1, t0, t1


def BlockFFT(M,nbptFFT,srate):
    """ FFT over block of data
        input : M : matrix of signals
                nbptFFT : integer point of FFT
                srate : integer sampling rate
        output : f : frequency axis
                M : FFT modulus
    """
    # axe frequence
    freq=np.linspace(0.0,float(srate),nbptFFT)
    freq=np.reshape(freq,nbptFFT)
    sfft=sc.fft(np.transpose(M),nbptFFT)
    mfft=np.abs(sfft)/nbptFFT
    mffttrans=np.transpose(mfft)  # transpose module FFT
    #
    f=freq[0:nbptFFT/2]
    M=mffttrans[0:nbptFFT/2,:]
    return f,M

def generate_wavelet_fourier(len_wavelet,
            f_start,
            f_stop,
            deltafreq,
            sampling_rate,
            f0,
            normalisation,
            ):
    """
    Compute the wavelet coefficients at all scales and makes its Fourier transform.
    When different signal scalograms are computed with the exact same coefficients,
        this function can be executed only once and its result passed directly to compute_morlet_scalogram

    Output:
        wf : Fourier transform of the wavelet coefficients (after weighting), Fourier frequencies are the first
    """
    # compute final map scales
    scales = f0/arange(f_start,f_stop,deltafreq)*sampling_rate
    # compute wavelet coeffs at all scales
    xi=arange(-len_wavelet/2.,len_wavelet/2.)
    xsd = xi[:,newaxis] / scales
    wavelet_coefs=exp(complex(1j)*2.*pi*f0*xsd)*exp(-power(xsd,2)/2.)
    weighting_function = lambda x: x**(-(1.0+normalisation))
    wavelet_coefs = wavelet_coefs*weighting_function(scales[newaxis,:])
    # Transform the wavelet into the Fourier domain
    #~ wf=fft(wavelet_coefs.conj(),axis=0) <- FALSE
    wf=fft(wavelet_coefs,axis=0)
    wf=wf.conj() # at this point there was a mistake in the original script
    return wf

def compute_morlet_scalogram(ana,
            f_start=5.,
            f_stop=100.,
            deltafreq = 1.,
            sampling_rate = 200.,
            f0=2.5,
            normalisation = 0.,
            wf=None
            ):
    """

    Input:
    ana: AnalogSignal
    f_start, f    i=int(ThresholdAfterTMS*srate)
    j=int((deltaAfterTMS+ThresholdAfterTMS)*srate)
    l0=int(blocksize/2)+i
    l1=int(blocksize/2)+j
    M=channels[ l0:l1, :]_stop, deltafreq : Frequency start stop and step at which the scalogram is computed
    samplingrate : time samplingrate of the scalogram
    f0 : central frequency of the Morlet wavelet.  The Fourier spectrum of
        the Morlet wavelet appears as a Gaussian centered on f0.
        It is also used as the wavelet characteristic frequency.
        Low f0 favors time precision of the scalogram while high f0 favors frequency precision
    normalisation : positive value favors low frequency components

    wf: if it is not None, it will ignore all other parameters and compute the map
        assuming wf is the Fourier transform of the wavelet_coefs

    Output:
    wt: complex array with wavelet (time is the first dimension, frequency the second)

    Note : this code is a simplification and correction of the full wavelet package (cwt.py)
    orinally proposed by Sean Arms (http://github.com/lesserwhirls)

    """
    print "debut compute_morlet_scalogram"
    print "ana = ", ana.shape," srate =",sampling_rate
    print ana[0:5]
    #Reduce signal to time limits
    sig=ana
    wf = generate_wavelet_fourier(len(sig),max(f_start,deltafreq),min(f_stop,sampling_rate/2.),deltafreq,sampling_rate,f0,normalisation)
    # Transform the signal into the Fourier domain
    sigf=fft(sig)
    # Convolve (mult. in Fourier space)
    #~ wt_tmp=ifft(wf*sigf[newaxis,:],axis=1)
    wt_tmp=ifft(sigf[:,newaxis]*wf,axis=0)
    # shift output from ifft
    wt = fftshift(wt_tmp,axes=[0])
    return wt


def CreatTimeTMS(blocksize,srate):
    """ calcul axe des temps pour le TMS"""
    dt=1.0/srate               # periode
    t=range(blocksize)          # taille d un bloc
    t=np.array(t, dtype="single") # conversion en float
    t=t-blocksize/2.       # zero au milieu de l axe
    t=t*dt                    # pour obtenir temps en seconde
    m, n=t.shape                  # dimension du tableau
    t=t.reshape( n)        #  arrange en une dimension
    return t
    i=int(ThresholdAfterTMS*srate)
    j=int((deltaAfterTMS+ThresholdAfterTMS)*srate)
    l0=int(blocksize/2)+i
    l1=int(blocksize/2)+j
    M=channels[ l0:l1, :]

def coefPassBand(AlphaMin, AlphaMax, srate):
        b= [] # store filter coefficient
        wl = 2.0*AlphaMin/srate*pi              # alpha min
        wh = 2.0*AlphaMax/srate*pi            # alpha max
        L = 512						# Set number of weights as 128
        bn = np.zeros(L)

        for i in xrange(0,L):					# Generate bandpass weighting function
            n = i-  L/2 						# Make symmetrical
            if n == 0:
                bn[i] = wh/pi - wl/pi;
            else:
                bn[i] = (sin(wh*n))/(pi*n) - (sin(wl*n))/(pi*n)   #	Filter impulse response

        bn = bn*sc.kaiser(L,5.2)  # apply Kaiser window, alpha= 5.2
        b.append(bn)
        return bn

def create_wavelet(signal,srate,fstart,fstop,deltafreq):
    print "debut create_wavelet"
    print "signal = ", signal.shape," srate =",srate
    print signal[0:5]
    wt=compute_morlet_scalogram(signal,
                    f_start=fstart,
                    f_stop=fstop,
                    deltafreq = deltafreq,
                    sampling_rate = srate,
                    f0=2,
                    normalisation = 0.,
                    wf=None
                    )
    print "apres compute scalogramwt  dim wt = ",wt.shape
    module=abs(wt).transpose()
    phase=sc.angle(wt).transpose()
    print "dim module = ",module.shape, " dim phase = ",phase.shape
    print "module = ", module[0,0:5]
    print "phase = ", phase[0,0:5]
    return wt,module,phase


def correlationM_Ref( M,
                     tau_max,
                     ref,
                     blocksize,srate,temps,ThresholdAfterTMS,deltaAfterTMS,
                     dt,#scale for time
                     nbchannel, #number of channels
                     nbblock):#nombre d'experiences

    i=int(ThresholdAfterTMS*srate)
    j=int((deltaAfterTMS+ThresholdAfterTMS)*srate)
    l0=int(blocksize/2)+i
    l1=int(blocksize/2)+j
    M_tau_moy=copy(M[:,:,l0:l1])
    Mshift=copy(M[:,:,l0:l1]) #in order to really duplicate measurements
    nb_voies=nbchannel
    tau=0
    M_tau=copy(Mshift)
#calcul de la crosscorrelation entre la ref et les signaux.
    for tau in range(tau_max):
        longueur=int(deltaAfterTMS/dt)
        delta_tau=int(longueur-tau)
        Mshift[:,:,0:delta_tau]=M[:,:,tau:longueur]
        Mshift[:,:,delta_tau:longueur]=0
        for i in range(nbchannel):
            for j in range(nbblock):
                y=Mshift[ref,j,:]
                x=M[i,j,l0:l1]
                M_tau[i,j,tau]=sc.correlate(x,y)
    return M_tau

def sinogramme(v):
    sup=np.max(np.max(v))
    inf=np.min(np.min(v))
    Z=(v-inf)/(sup-inf)
    Z=(Z-0.5)*2.0
    m,n=v.shape
    for i in  range(n):
        Z[:,i]=Z[:,i]+i
    return Z


def raz_artefact(t,v,ThresholdBeforeTMS,ThresholdAfterTMS,srate):
    m,n=v.shape
    i=int(ThresholdBeforeTMS*srate)
    j=int(ThresholdAfterTMS*srate)
    l0=int(m/2)-i
    l1=int(m/2)+j
    for k in range(n):
        v[ l0:l1, k]=0
    return v


